/***************************************************************************
				HandleEqs.cpp
                             -------------------
Copyright (c) 2009-2010 
Author: Kishor D. Bhalerao (kishor8dm@gmail.com)
Contributors: 

This file is associated with the Ph.D. dissertation,"ON METHODS FOR EFFICIENT
AND ACCURATE DESIGN AND SIMULATION OF MULTIBODY SYSTEMS", Bhalerao K.D,
Rensselaer Polytechnic Institute, 2010.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1.) Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2.) Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

3.) The name of the authors, their employers, or sponsoring organizations may
not be used to endorse or promote products derived from this software without
specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
EVENT SHALL THE AUTHORS, THEIR EMPLOYERS, OR SPONSORING ORGANIZATIONS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#include "HandleEqs.h"
#include "MatOps.h"


// 1 The following part is associated with the system without the Thermostat
// formHandleEquations: Calculate psi13 and psi23 for each individual body at the Leaf level

void HandleEqs::formHandleEquations(const DcaBodyData *Mbs){
	
	MatOp op;
	//std::cout << "0.1"<<std::endl;
	system_.realize(state_,Stage::Velocity);
	for (int i = 0 ; i<matter_.getNumBodies(); i++){
		MobilizedBody bd = matter_.getMobilizedBody(MobilizedBodyIndex(i));
		Real mass1 = bd.getBodyMass(state_);
		Inertia Inet1 = bd.getBodyInertiaAboutBodyOrigin(state_);
		Vec3 Ang1 = bd.getBodyAngularVelocity(state_);

	}
	
	system_.realize(state_,Stage::Dynamics);
	/*std::cout << system_.getMobilityForces(state_, Stage::Dynamics) << std::endl << std::endl;
	std::cout << system_.getRigidBodyForces(state_,Stage::Dynamics) << std::endl << std::endl;*/

	//std::cout << "0.2"<<std::endl;
	
	int nBodies = matter_.getNumBodies();

    for(int i = 0;i<nBodies-1;i++){
		
		const MobilizedBody& body = matter_.getMobilizedBody(MobilizedBodyIndex(Mbs[i].Id)); 
		
		// Note, matrix M_C_B is constant for each body. There is once excess matrix
		// multiplication here at each time step for each rigid body in the system
		Mat33 M_C_N = (~body.getOutboardFrame(state_).R())*(~body.getBodyRotation(state_).asMat33());

		//Get Angular Velocity in Newtonian frame and expressed in Mobilizer Basis
		N_W_K[i] = M_C_N*body.getBodyAngularVelocity(state_);

		// External spatial force acting on the origin (not center of mass) of this body in Newtonian Frame.
		// Available at stage dynamics
		const SpatialVec& RigidBodyForce = system_.getRigidBodyForces(state_,Stage::Dynamics)[Mbs[i].Id];
	
		//std::cout << RigidBodyForce << std::endl;
		// This part can be further improved 
		// This an important part in MD
		Vec3 f1 = Mbs[i].iIner*(M_C_N*RigidBodyForce[0]-(Mbs[i].BtoCM)%(M_C_N*RigidBodyForce[1])); // inv(Inertia) * External Torque
		Vec3 f2 = M_C_N*RigidBodyForce[1]/body.getBodyMass(state_);		  // inv(mass) * External Force

		Vec3 top = -Mbs[i].iIner*(N_W_K[i]%(Mbs[i].Iner*N_W_K[i])); // %: operator in SmallMatrixMixed, cross product, inv(Inertia) * (w cross IW)
		
		Vec6 psi_13(0);
		Vec3 tmpVec = top+f1;
		op.toVec6(tmpVec,f2,psi_13);
		
		Vec3 Z3x1(0,0,0);
		Vec3 tmpVec1 = op.CrossProduct(N_W_K[i],op.CrossProduct(N_W_K[i],Mbs[i].cm2H1)); // omega Cross ( W Cross R)
		Vec3 tmpVec2 = op.CrossProduct(N_W_K[i],op.CrossProduct(N_W_K[i],Mbs[i].cm2H2));
		psi13[i] = Mbs[i].SfH1*psi_13 + op.toVec6(Z3x1,tmpVec1);
		psi23[i] = Mbs[i].SfH2*psi_13 + op.toVec6(Z3x1,tmpVec2);
		//psi13[i] = Mbs[i].SfH1*psi_13 + op.toVec6(Z3x1,op.CrossProduct(N_W_K[i],op.CrossProduct(N_W_K[i],Mbs[i].cm2H1)));
		//psi23[i] = Mbs[i].SfH2*psi_13 + op.toVec6(Z3x1,op.CrossProduct(N_W_K[i],op.CrossProduct(N_W_K[i],Mbs[i].cm2H2)));
	
		//std::cout << i << " 13= " << psi13[i] << std::endl;
		//std::cout << i << " 23 = " << psi23[i] << std::endl;
	} 

} // 1


// The method is added Aug 15 2010 by Ali to cinsider the effect of the thermostat to the psi13 and phi 23
void HandleEqs::formThermostatHandleEqs(const DcaBodyData *Mbs,const int **FreeJoints){

	// The second element of the Z is the same as the first element of the ChainState and 
	// either of them can be used to find the thermostat generalized force = -Z[1] MV

	/*std::cout << "Z=" << state_.getZ() << std::endl;
	std::cout << "ZDot=" << state_.getZDot() << std::endl;
	std::cout << "ChainState= " << Therm_.getChainState(state_)  << std::endl;*/

	Vector V = state_.getU();
	//std::cout << V << std::endl;
	Vector MV ;
    matter_.calcMV(state_,V,MV);
	//std::cout << state_.getTime()<< std::endl; 
		
	Real Z0 = state_.getZ()[1];
	Vector ThermGeneralizedForce = -Z0 * MV; 

    //std::cout << ThermGeneralizedForce << endl;
	

	//std::cout << Z0 << std::endl ; 

	
	//cout << state_.getTime() << "\t" << Therm_.getCurrentTemperature(state_)  << std::endl;
	//std::cout << Z0 << std::endl;

	int NBodies = matter_.getNumBodies();
	//	
	// first body: 
	Mat66 PJ0(0);
	PJ0(0,0) = PJ0(1,1) = PJ0(2,2) = PJ0(3,3) = PJ0(4,4) = PJ0(5,5) = 1 ;

		
	// For the Revolute Joint
	  Vec6 *PJ = new Vec6[NBodies-1];	  

	for (int i = 0 ; i < NBodies-1 ; i++){
		PJ[i] = (0); 
	}

	// FreeJoints_(new int[matter.getNumBodies()-1])
	// Therefore, the (*FreeJoints_)[i] and PJ[i] are defined between bodies i and i+1. i = 0: ground
	for (int i = 1 ; i < NBodies-1 ; i++) {
		if ((*FreeJoints)[i] == 1){
			PJ[i](2) = 1 ;
		}
	}
		
	 // *FreeJoints[1] is associated with the joint between the first and the second bodies, zeroth body is the ground

	// Responsible for the correction of the psi13 nad psi23 of the first body START****
	int i ; // index of the body in simtk, i=0: ground

	i = 1 ;
		// IMPORTANT HINT IN INDEX:
		// The indecies are tricky here, in simbody the index(0) is the ground, however herein the index(0)
		// is the first body therefore:
		// 1) psi13[0] belongs to the first body not the Ground
		// 2) matter_.getMobilizedBody(MobilizedBodyIndex(1)); refers to the first body which is not ground
		

		Vec6 psi13_temp = getpsi13()[i-1]; // properties of the Parent = the current body(1), it has a child (body2)
		Vec6 psi23_temp = getpsi23()[i-1]; // properties of the Parent

       					
		const MobilizedBody& Parent = matter_.getMobilizedBody(MobilizedBodyIndex(i)); 
		const MobilizedBody& Child = matter_.getMobilizedBody(MobilizedBodyIndex(i+1));
		
        Mat33 Fp_C_Mp = Parent.getMobilizerTransform(state_).R();
		MatOp op;
		Mat33 Z(0);
		Mat66 Fp_C_Mp_sp = op.toMat66(Fp_C_Mp,Z,Z,Fp_C_Mp);
		//std::cout << Fp_C_Mp_sp << std::endl;
		Mat66 PJ_inward_Mp = ~(Fp_C_Mp_sp) * PJ0; // PJ0 = Identity matrix 6 by 6
		
		
		Mat33 Fc_C_Bp = ~Child.getInboardFrame(state_).R();
		Mat33 Bp_C_Mp = Parent.getOutboardFrame(state_).R();
		Mat33 Fc_C_Mp = Fc_C_Bp * Bp_C_Mp;
		Mat66 Fc_C_Mp_sp = op.toMat66(Fc_C_Mp,Z,Z,Fc_C_Mp);
		Vec6 PJ_outward_Mp = ~(Fc_C_Mp_sp)  * PJ[i];

		Vec6 GenForce0;
		for (int j = 0 ; j < 6; j++){

			GenForce0(j) = ThermGeneralizedForce[j];
			//std::cout << GenForce0(j) << std::endl;
			//std::cout << ThermGeneralizedForce[j] << std::endl;

		}

		/*GenForce0(3) = 0.0;
		GenForce0(4) = 0.0;
		GenForce0(5) = 0.0;*/

		//cout << PJ_inward_Mp * GenForce0 << endl ;

		
		//std::cout << Mbs[i-1].psi11 * PJ_inward_Mp * GenForce0 - Mbs[i-1].psi12 * PJ_outward_Mp * ThermGeneralizedForce[i+5]  << endl ;
		//std::cout << Mbs[i-1].psi21 * PJ_inward_Mp * GenForce0 - Mbs[i-1].psi22 * PJ_outward_Mp * ThermGeneralizedForce[i+5] << endl ;

		psi13[i-1] = psi13_temp + Mbs[i-1].psi11 * PJ_inward_Mp * GenForce0 - Mbs[i-1].psi12 * PJ_outward_Mp * ThermGeneralizedForce[i+5] ;
		psi23[i-1] = psi23_temp + Mbs[i-1].psi21 * PJ_inward_Mp * GenForce0 - Mbs[i-1].psi22 * PJ_outward_Mp * ThermGeneralizedForce[i+5] ;

		/*std::cout << psi13[i-1] << std::endl; 
		std::cout << psi23[i-1] << std::endl; */
		

	//// FINISH***	


   // Responsible for the correction of the psi13 and psi23 of all bodies except the first and the last one	
   // i here is the index of the body in simtk, i = 0: ground
	for (int i = 2 ; i <= NBodies - 2 ; i++) {

		// IMPORTANT HINT IN INDEX:
		// The indecies are tricky here, in simbody the index(0) is the ground, however herein the index(0)
		// is the first body therefore:
		// 1) psi13[0] belongs to the first body NOT the Ground
		// 2) matter_.getMobilizedBody(MobilizedBodyIndex(1)); refers to the first body which is not ground
		

		Vec6 psi13_temp = getpsi13()[i-1]; // properties of the Parent = the current body(i) is Parent, it has a child(i+1)
		Vec6 psi23_temp = getpsi23()[i-1]; 
        					
		const MobilizedBody& Parent = matter_.getMobilizedBody(MobilizedBodyIndex(i)); // current body
		const MobilizedBody& Child = matter_.getMobilizedBody(MobilizedBodyIndex(i+1));
		
        Mat33 Fp_C_Mp = Parent.getMobilizerTransform(state_).R();
		MatOp op;
		Mat33 Z(0);
		Mat66 Fp_C_Mp_sp = op.toMat66(Fp_C_Mp,Z,Z,Fp_C_Mp);
		Vec6 PJ_inward_Mp = ~(Fp_C_Mp_sp) * PJ[i-1]; // Since  Fp_C_Mp is about the z-axis, and PJ = [0 0 1 0 0 0], PJ_inward_Mp = PJ 

		Mat33 Fc_C_Bp = ~Child.getInboardFrame(state_).R();
		Mat33 Bp_C_Mp = Parent.getOutboardFrame(state_).R();
		Mat33 Fc_C_Mp = Fc_C_Bp * Bp_C_Mp;
		Mat66 Fc_C_Mp_sp = op.toMat66(Fc_C_Mp,Z,Z,Fc_C_Mp);
		Vec6 PJ_outward_Mp = ~(Fc_C_Mp_sp)  * PJ[i];


		//std:: cout << Mbs[i-1].psi11 * PJ_inward_Mp * ThermGeneralizedForce[i+4] - Mbs[i-1].psi12 * PJ_outward_Mp * ThermGeneralizedForce[i+5]  << endl;
		//std::cout << Mbs[i-1].psi21 * PJ_inward_Mp * ThermGeneralizedForce[i+4] - Mbs[i-1].psi22 * PJ_outward_Mp * ThermGeneralizedForce[i+5] << endl ;
		//std:: cout << PJ_inward_Mp * ThermGeneralizedForce[i+4] << endl;
		//std::cout <<  PJ_inward_Mp * ThermGeneralizedForce[i+4]  << endl ;
		
		//std:: cout << Mbs[i-1].psi11   << endl;
		//std::cout << Mbs[i-1].psi21  << endl ;
		
		psi13[i-1] = psi13_temp + Mbs[i-1].psi11 * PJ_inward_Mp * ThermGeneralizedForce[i+4] - Mbs[i-1].psi12 * PJ_outward_Mp * ThermGeneralizedForce[i+5] ;
		psi23[i-1] = psi23_temp + Mbs[i-1].psi21 * PJ_inward_Mp * ThermGeneralizedForce[i+4] - Mbs[i-1].psi22 * PJ_outward_Mp * ThermGeneralizedForce[i+5] ;

		/*std::cout << psi13[i-1] << std::endl; 
		std::cout << psi23[i-1] << std::endl; */
		
	}

	

  // Correction for the last body //START****
	 i = NBodies - 1 ;  

		// IMPORTANT HINT IN INDEX:
		// The indecies are tricky here, in simbody the index(0) is the ground, however herein the index(0)
		// is the first body therefore:
		// 1) psi13[0] belongs to the first body not the Ground
		// 2) matter_.getMobilizedBody(MobilizedBodyIndex(1)); refers to the first body which is not ground
		
	  psi13_temp = getpsi13()[i-1]; // properties of the Parent, this body does not have a child
	  psi23_temp = getpsi23()[i-1]; // properties of the Parent
 //       					
	  const MobilizedBody& Last = matter_.getMobilizedBody(MobilizedBodyIndex(i)); 	

      Mat33 LFp_C_Mp = Last.getMobilizerTransform(state_).R();
	  Mat66 LFp_C_Mp_sp = op.toMat66(LFp_C_Mp,Z,Z,LFp_C_Mp);
	  Vec6  LPJ_inward_Mp = ~(LFp_C_Mp_sp) * PJ[i-1];

	  psi13[i-1] = psi13_temp + Mbs[i-1].psi11 * LPJ_inward_Mp * ThermGeneralizedForce[i+4];
	  psi23[i-1] = psi23_temp + Mbs[i-1].psi21 * LPJ_inward_Mp * ThermGeneralizedForce[i+4] ;
	
	//finish****

}

	








   


	


